/*
 * BatteryMonitor.cpp
 *
 *  Created on: Dec 30, 2013
 *      Author: Adam Stroud
 *
 *		Called from MUL8.cpp
 *      This class checks the voltage across motors 1 and 2, averages their values and prints them to the console.
 *
 *  Update 1/2/2014 by Kellen Carey
 *  	Is able to read voltage across motor 1 consistently as 24-25 until motors begin moving
 *  	After motors begin moving, voltage readout drops to zero.
 *  	This also coincides with very large readouts of present motor positions.
 *  	Before motors begin moving, present position readouts are relatively close to actual position
 *  		Not close enough to be considered accurate
 *  	After motors begin moving, position readout jumps to five or six digit figures.
 */

#include "BatteryMonitor.h"
#include "Dyna.h"
#include <string>
#include <sstream>
#include <stdlib.h>

Dyna battery_monitor;


BatteryMonitor::BatteryMonitor() {
	// TODO Auto-generated constructor stub

	// Initialize USB2Dynamixel
	battery_monitor.dxl_initialize(0,1);
	// Initialize the motors
	for (int i = 0; i < 24; i++)
	{
		battery_monitor.dxl_write_byte(i+1, RETURN_DELAY, 250);
	}

}

BatteryMonitor::~BatteryMonitor() {
	// TODO Auto-generated destructor stub
	battery_monitor.dxl_terminate();
}

/**
 * called from MUL8.cpp
 * finds voltage across 2 motors and therefore the battery itself. Returns the (float) voltage to MUL8.cpp for decision making.
 */
float BatteryMonitor::step(bool IsWorking) {

	float voltage=0;
	if(IsWorking)
		std::cout << " Can't get battery voltage!! " << std::endl;

	else
		/* go through motors 1 and 2, get their voltages and sum them
		for (int i = 0; i < BATTERY_MOTORS; i++)
		{

			// sum voltage across motors
			 *
			 */
		// TODO Changed dxl_read_word to dxl_read_byte (Voltage is one byte long, not two)
		// Still giving numbers around twice what they should be.
		voltage= (battery_monitor.dxl_read_byte(12, PRESENT_VOLTAGE))/10;

		//}
		//voltage= (voltage/10); //get average voltage of 2 hip motors. given in millivolts, so divide by 10

		// tell the user the voltage
		std::cout << "  Battery Voltage: " << voltage <<  std::endl;


	return voltage;
}


